/*
 *  matrix.c
 *  Renderbunny
 *
 *  Created by Holmes Futrell on 1/21/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include "matrix.h"
#include <math.h>
#include <stdio.h>

#pragma mark static methods
static mat4 rb_mat4_cofactor( const mat4 *mat );
static mat3 rb_mat4_minor( const mat4 *mat, int minor_row, int minor_column );
static float rb_mat3_det( const mat3 *mat );
static float rb_mat4_det( const mat4 *mat );
static mat4 rb_mat4_mul_scalar( const mat4 *mat, float scalar );

void rb_describe_mat4(mat4 mat) {

	int row, col;
	for (row=0; row<4; row++) {
		for (col=0; col<4; col++) {			
			printf("%3.5f ", mat.entries[M4E(row, col)] );
		}
		printf("\n");
	}
	
}

#pragma mark -

static mat3 rb_mat3_mul_scalar( const mat4 *mat, float scalar ) {
	mat3 result;
	int i;
	for (i=0; i<9; i++) {
		result.entries[i] = scalar * mat->entries[i];
	}
	return result;
}

static mat4 rb_mat4_mul_scalar( const mat4 *mat, float scalar ) {
	mat4 result;
	int i;
	for (i=0; i<16; i++) {
		result.entries[i] = scalar * mat->entries[i];
	}
	return result;
}

mat4 rb_mat4_inverse( const mat4 *mat ) {
	float s = 1.0f / rb_mat4_det(mat);
	
	mat4 cofactor = rb_mat4_cofactor(mat);
	mat4 adjugate = rb_mat4_transpose(&cofactor);
	mat4 mult = rb_mat4_mul_scalar(&adjugate, s);
	rb_describe_mat4(mult);

	
	return mult;
}

static mat4 rb_mat4_cofactor( const mat4 *mat ) {
	mat4 result;
	int row, col;
	for (row=0; row<4; row++) {
		for (col=0; col<4; col++) {
			float parity = (float)((row + col) % 2 ? -1.0f : 1.0f);
			mat3 minorMatrix = rb_mat4_minor(mat, row, col);
			result.entries[M4E(row, col)] = parity * rb_mat3_det(&minorMatrix);
		}
	}
	return result;
}

/*
	For a row, col of a 4x4 matrix, gives the minor 3x3 matrix (matrix with row/col absent)
	Used for computing determinant
 */
static mat3 rb_mat4_minor( const mat4 *mat, int minor_row, int minor_col ) {
	mat3 result;
	int row, col;
	for (row=0; row<4; row++) {
		if ( row == minor_row )
			continue;
		int dst_row = row < minor_row ? row : (row - 1);
		for (col=0; col<4; col++) {
			if ( col == minor_col )
				continue;
			int dst_col = col < minor_col ? col : (col - 1);
			result.entries[M3E(dst_row, dst_col)] = mat->entries[M4E(row, col)];
		}
	}
	return result;
}

float rb_mat3_det( const mat3 *mat ) {

	const float *e = mat->entries;
	// compute determinants of minor matrices
	float m00 = e[M3E(1,1)] * e[M3E(2,2)] - e[M3E(1,2)] * e[M3E(2,1)];
	float m01 = e[M3E(1,0)] * e[M3E(2,2)] - e[M3E(1,2)] * e[M3E(2,0)];
	float m02 = e[M3E(1,0)] * e[M3E(2,1)] - e[M3E(1,1)] * e[M3E(2,0)];

	return	  e[M3E(0,0)] * m00 \
			- e[M3E(0,1)] * m01 \
			+ e[M3E(0,2)] * m02;
}

float rb_mat4_det( const mat4 *mat ) {
	mat3 m00 = rb_mat4_minor(mat, 0, 0);
	mat3 m01 = rb_mat4_minor(mat, 0, 1);
	mat3 m02 = rb_mat4_minor(mat, 0, 2);
	mat3 m03 = rb_mat4_minor(mat, 0, 3);
	return		  mat->entries[M4E(0,0)] * rb_mat3_det(&m00) 
				- mat->entries[M4E(0,1)] * rb_mat3_det(&m01) 
				+ mat->entries[M4E(0,2)] * rb_mat3_det(&m02) 
				- mat->entries[M4E(0,3)] * rb_mat3_det(&m03);
}

mat4 rb_mat4_transpose( const mat4 *mat ) {
	mat4 result;
	int row, col;
	for (row=0; row<4; row++) {
		for (col=0; col<4;col++) {
			result.entries[M4E(row, col)] = mat->entries[M4E(col, row)];
		}
	}
	return result;
}

#pragma mark -

mat4 rb_mat4_from_mat3( mat3 mat ) {
	mat4 result;
	int row, col;
	for (row=0; row<3; row++) {
		for (col=0; col<3; col++) {			
			result.entries[M4E(row, col)] = mat.entries[M3E(row, col)];
		}
	}
	// fill in the rest of the entries
	result.entries[M4E(0,3)] = 0.0f;
	result.entries[M4E(1,3)] = 0.0f;
	result.entries[M4E(2,3)] = 0.0f;
	result.entries[M4E(3,0)] = 0.0f;
	result.entries[M4E(3,1)] = 0.0f;
	result.entries[M4E(3,2)] = 0.0f;
	result.entries[M4E(3,3)] = 1.0f;
	return result;
}


mat3 rb_mat3_from_mat4( mat4 mat ) {
	mat3 result;
	int row, col;
	for (row=0; row<3; row++) {
		for (col=0; col<3; col++) {			
			result.entries[M3E(row, col)] = mat.entries[M4E(row, col)];
		}
	}
	return result;
}

mat4 rb_mat4_zero(void) {
	mat4 result;
	int i;
	for (i=0; i<16; i++) {
		result.entries[i] = 0.0;
	}
	return result;	
}

mat4 rb_mat4_translate(vec3 t) {
	mat4 result = rb_mat4_identity();
	result.entries[M4E( 0, 3 )] = t.coords[0];
	result.entries[M4E( 1, 3 )] = t.coords[1];
	result.entries[M4E( 2, 3 )] = t.coords[2];
	return result;
}

mat4 rb_mat4_identity() {
	mat4 result;
	int row, col;
	for (row=0; row<4; row++) {
		for (col=0; col<4; col++) {			
			result.entries[row + 4 * col] = (row == col) ? 1.0f : 0.0f;
		}
	}
	return result;
}

mat4 rb_mat4_mul( mat4 a, mat4 b) {
	mat4 result;
	int row, col, k;
	for (row=0; row<4; row++) {
		for (col=0; col<4; col++) {			
			float accum = 0.0;
			for (k=0; k<4; k++) {
				accum += a.entries[M4E(row, k)] * b.entries[M4E(k, col)];
			}
			result.entries[M4E(row, col)] = accum;
		}	
	}
	return result;
	
}
